#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/follow_waypoints.hpp"
#include <chrono>

using FollowWaypoints = nav2_msgs::action::FollowWaypoints;
using namespace std::chrono_literals;

class NavigateWaypointFollower : public rclcpp::Node{
private:
    rclcpp_action::Client<FollowWaypoints>::SharedPtr m_waypoint_follower;

public:
    NavigateWaypointFollower():Node("waypoint_follower_node"){
        RCLCPP_INFO(this->get_logger(),"节点waypoint_follower_node已启动");
        m_waypoint_follower = rclcpp_action::create_client<FollowWaypoints>(this,"/follow_waypoints");
    }

    void send_goals(){
        while(!m_waypoint_follower->wait_for_action_server(5s))
        {
            RCLCPP_WARN(this->get_logger(),"等待action服务端上线...");
        }

        std::vector<geometry_msgs::msg::PoseStamped> poses;
        geometry_msgs::msg::PoseStamped pose1;
        pose1.header.stamp = this->get_clock()->now();
        pose1.header.frame_id = "map";
        pose1.pose.position.x = 1.0;
        pose1.pose.position.y = 0.0;
        pose1.pose.orientation.w = 1.0;
        poses.emplace_back(pose1);
        geometry_msgs::msg::PoseStamped pose2;
        pose2.header.stamp = this->get_clock()->now();
        pose2.header.frame_id = "map";
        pose2.pose.position.x = 2.0;
        pose2.pose.position.y = 0.0;
        pose2.pose.orientation.w = 1.0;
        poses.emplace_back(pose2);
        geometry_msgs::msg::PoseStamped pose3;
        pose3.header.stamp = this->get_clock()->now();
        pose3.header.frame_id = "map";
        pose3.pose.position.x = 2.0;
        pose3.pose.position.y = 0.0;
        pose3.pose.orientation.w = 1.0;
        poses.emplace_back(pose3);
        auto goals = FollowWaypoints::Goal();
        goals.poses = poses;
        
        auto send_goal_option = rclcpp_action::Client<FollowWaypoints>::SendGoalOptions();
        send_goal_option.goal_response_callback = [this](rclcpp_action::ClientGoalHandle<FollowWaypoints>::SharedPtr goal_handle){
            if(goal_handle){
                RCLCPP_INFO(this->get_logger(),"目标点都已经被服务端接收");
            }
        };
        send_goal_option.feedback_callback = [this](rclcpp_action::ClientGoalHandle<FollowWaypoints>::SharedPtr goal_handle,const std::shared_ptr<const rclcpp_action::ClientGoalHandle<FollowWaypoints>::Feedback> feedback){
            (void)goal_handle;
            RCLCPP_INFO(this->get_logger(),"当前途经点:%d",feedback->current_waypoint);
        };
        send_goal_option.result_callback = [this](const rclcpp_action::ClientGoalHandle<FollowWaypoints>::WrappedResult & result){
            if(result.code == rclcpp_action::ResultCode::SUCCEEDED)
            {
                RCLCPP_INFO(this->get_logger(),"处理成功");
            }
        };
        m_waypoint_follower->async_send_goal(goals,send_goal_option);
    }
};

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto node = std::make_shared<NavigateWaypointFollower>();
    node->send_goals();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}